﻿using System;
using System.Collections.Generic;
using System.Runtime.InteropServices;
using FLMScan.Geometry;
using System.Drawing;
using FLMScan.WorldCoordinateReconstruction.Riedmaier;
using FLMScan.Parameters;
using FLMScan.Capture;

namespace FLMScan.OpenCV
{
    class CalibrationWrapperOpenCV
    {
        // TBD: find out why CvCalibrateCamera2 doesn't work with OpenCV 2.1
        // private const string CV_LIBRARY = ".\\3rdParty\\OpenCV\\cv210.dll";
        private const string CV_LIBRARY_210 = ".\\3rdParty\\OpenCV\\cv210.dll";
        private const string CV_LIBRARY = ".\\3rdParty\\OpenCV\\cv100.dll";        
        
        public static void FindExtrinsicCameraParams2(FlmMatrix ledWorldPoints, FlmMatrix webcamPoints, FlmMatrix camMatrix, FlmMatrix distortion
            , out FlmMatrix rotation, out FlmMatrix translationVector)
        {
            CvMat rvec = MatrixWrapperOpenCV.CvCreateMat(3, 1, OpenCVConstants.CV_64FC1);
            CvMat trans = MatrixWrapperOpenCV.CvCreateMat(3, 1, OpenCVConstants.CV_64FC1);

            CvMat rot = MatrixWrapperOpenCV.CvCreateMat(3, 3, OpenCVConstants.CV_64FC1);
            CvMat dummy = MatrixWrapperOpenCV.CvCreateMat(9, 3, OpenCVConstants.CV_64FC1);

            CvMat worldPoints = ledWorldPoints.AsCvMat();
            CvMat imagePoints = webcamPoints.AsCvMat();
            CvMat cameraMatrix = camMatrix.AsCvMat();
            CvMat distortionCoefficients = distortion.AsCvMat();

            // calculate rotation vector and translation vector 
            // that brings points from the model coordinate system to the camera coordinate system
            CvFindExtrinsicCameraParams2(ref worldPoints, ref imagePoints, ref cameraMatrix
                , ref distortionCoefficients, ref rvec, ref trans);
            // convert rotation vector to rotation matrix
            CvRodrigues2(ref rvec, ref rot, ref dummy);

            translationVector = FlmMatrix.FromCvMat(trans);
            rotation = FlmMatrix.FromCvMat(rot);

            MatrixWrapperOpenCV.CvReleaseMat(ref rvec);
            MatrixWrapperOpenCV.CvReleaseMat(ref trans);
            MatrixWrapperOpenCV.CvReleaseMat(ref rot);
            MatrixWrapperOpenCV.CvReleaseMat(ref dummy);
            MatrixWrapperOpenCV.CvReleaseMat(ref worldPoints);
            MatrixWrapperOpenCV.CvReleaseMat(ref imagePoints);
            MatrixWrapperOpenCV.CvReleaseMat(ref cameraMatrix);
            MatrixWrapperOpenCV.CvReleaseMat(ref distortionCoefficients);
        }

        public static FlmMatrix Undistort(FlmMatrix source, FlmMatrix intrinsicCameraParametersMatrix, FlmMatrix distortion)
        {
            CvMat intrinsic_matrix = intrinsicCameraParametersMatrix.AsCvMat();
            CvMat distortion_coeffs = distortion.AsCvMat();

            FlmMatrix undistorted = new FlmMatrix(source.Rows, source.Columns);

            for (int i = 0; i < source.Rows; i++)
            {
                CvMat src = MatrixWrapperOpenCV.CvCreateMat(1, 1, OpenCVConstants.CV_64FC2);
                MatrixWrapperOpenCV.CvSet2D(ref src, 0, 0, new CvScalar(source[i, 0], source[i, 1], 0, 0));
                CvMat dst = MatrixWrapperOpenCV.CvCreateMat(1, 1, OpenCVConstants.CV_64FC2);

                CvUndistortPoints(ref src, ref dst, ref intrinsic_matrix, ref distortion_coeffs);

                CvScalar scalar = MatrixWrapperOpenCV.CvGet2D(ref dst, 0, 0);
                undistorted[i, 0] = scalar.val1;
                undistorted[i, 1] = scalar.val2;

                MatrixWrapperOpenCV.CvReleaseMat(ref src);
                MatrixWrapperOpenCV.CvReleaseMat(ref dst);
            }

            MatrixWrapperOpenCV.CvReleaseMat(ref intrinsic_matrix);
            MatrixWrapperOpenCV.CvReleaseMat(ref distortion_coeffs);

            return undistorted;
        }

        public static List<FlmPoint> RetrieveChessboardCorners(Bitmap chessboardBitmap, int innerChessboardCornersX, int innerChessboardCornersY)
        {
            List<FlmPoint> chessboardCorners = new List<FlmPoint>();
            CapturedFrame image = new CapturedFrame(chessboardBitmap);

            // create greyscale image 
            IplImage image_gray = CaptureWrapperOpenCV.CvCreateImage(new CvSize(image.Frame.width, image.Frame.height), (int)OpenCVConstants.IPL_DEPTH_8U, 1);
            IplImage originalImage = image.Frame;
            CaptureWrapperOpenCV.CvCvtColor(ref originalImage, ref image_gray, OpenCVConstants.CV_BGR2GRAY);

            CvPoint2D32f[] tmpCorners = new CvPoint2D32f[innerChessboardCornersX * innerChessboardCornersY];
            GCHandle h;
            IntPtr ptr = Convert1DArrToPtr(tmpCorners, out h);
            int corner_count = 0;
            CvSize board_size = new CvSize(innerChessboardCornersX, innerChessboardCornersY);
            int found = CvFindChessboardCorners(ref image_gray, board_size, ptr, ref corner_count, 0);

            if (found > 0)
            {
                // calculate sub pix accuracy
                CvFindCornerSubPix(ref image_gray, ptr, corner_count, new CvSize(11, 11), new CvSize(-1, -1), new CvTermCriteria(OpenCVConstants.CV_TERMCRIT_ITER, 30, 0));

                CvDrawChessboardCorners(ref originalImage, board_size, ptr, corner_count, found);

                // TBD: rather run through number of actually found corners ...
                int chessboardCornerCount = innerChessboardCornersX * innerChessboardCornersY;
                for (int j = 0; j < chessboardCornerCount; j++)
                {
                    chessboardCorners.Add(new FlmPoint(tmpCorners[j].x, tmpCorners[j].y));
                }
            }

            CaptureWrapperOpenCV.CvReleaseImage(ref image_gray);
            CaptureWrapperOpenCV.ReleaseHandel(h);

            return chessboardCorners;
        }

        public static IntrinsicCameraParameters CalibrateCamera(int calibrationImageCount, int innerChessboardCornersX, int innerChessboardCornersY, Dictionary<int, List<FlmPoint>> chessboardCorners, double chessboardWorldSquareSize, int imageWidth, int imageHeight)
        {
            IntrinsicCameraParameters intrinsicParameters = new IntrinsicCameraParameters();
            int chessboardCornerCount = innerChessboardCornersX * innerChessboardCornersY;
            CvMat object_points = MatrixWrapperOpenCV.CvCreateMat(1, calibrationImageCount * chessboardCornerCount, OpenCVConstants.CV_32FC2);
            CvMat image_points = MatrixWrapperOpenCV.CvCreateMat(1, calibrationImageCount * chessboardCornerCount, OpenCVConstants.CV_32FC2);
            CvMat point_counts = MatrixWrapperOpenCV.CvCreateMat(1, calibrationImageCount, OpenCVConstants.CV_32SC1);
            MatrixWrapperOpenCV.CvSet(ref point_counts, new CvScalar(chessboardCornerCount, 0, 0, 0));

            int counter = 0;
            for (int i = 0; i < calibrationImageCount; i++)
            {
                List<FlmPoint> currentChessboardCorners = chessboardCorners[i];

                int cornerCounter = 0;
                for (int j = 0; j < innerChessboardCornersY; j++)
                {
                    for (int k = 0; k < innerChessboardCornersX; k++)
                    {
                        MatrixWrapperOpenCV.CvSet2D(ref image_points, 0, counter, new CvScalar(currentChessboardCorners[cornerCounter].X, currentChessboardCorners[cornerCounter].Y, 0, 0));
                        MatrixWrapperOpenCV.CvSet2D(ref object_points, 0, counter, new CvScalar(j * chessboardWorldSquareSize, k * chessboardWorldSquareSize, 0, 0));
                        ++cornerCounter;
                        ++counter;
                    }
                }
            }

            CvMat camera_matrix = MatrixWrapperOpenCV.CvCreateMat(3, 3, OpenCVConstants.CV_64F);
            CvMat dist_coeffs = MatrixWrapperOpenCV.CvCreateMat(1, 4, OpenCVConstants.CV_64F);
            MatrixWrapperOpenCV.CvSetZero(ref camera_matrix);
            MatrixWrapperOpenCV.CvSetZero(ref dist_coeffs);

            CvMat extr_params = MatrixWrapperOpenCV.CvCreateMat(calibrationImageCount, 6, OpenCVConstants.CV_32FC1);
            CvMat rot_vects = new CvMat();
            CvMat trans_vects = new CvMat();

            // TBD: not sure what this code does...
            MatrixWrapperOpenCV.CvGetCols(ref extr_params, ref rot_vects, 0, 3);
            MatrixWrapperOpenCV.CvGetCols(ref extr_params, ref trans_vects, 3, 6);
            int flags = 0 | OpenCVConstants.CV_CALIB_FIX_PRINCIPAL_POINT;

            CvCalibrateCamera2(ref object_points, ref image_points, ref point_counts, new CvSize(imageWidth, imageHeight)
                , ref camera_matrix, ref dist_coeffs, ref rot_vects, ref trans_vects, flags);

            intrinsicParameters.FocalLengthX = MatrixWrapperOpenCV.CvGetReal2D(ref camera_matrix, 0, 0);
            intrinsicParameters.FocalLengthY = MatrixWrapperOpenCV.CvGetReal2D(ref camera_matrix, 1, 1);
            intrinsicParameters.PrimaryPointX = MatrixWrapperOpenCV.CvGetReal2D(ref camera_matrix, 0, 2);
            intrinsicParameters.PrimaryPointY = MatrixWrapperOpenCV.CvGetReal2D(ref camera_matrix, 1, 2);

            List<double> distortionCoefficients = new List<double>();
            distortionCoefficients.Add(MatrixWrapperOpenCV.CvGetReal2D(ref dist_coeffs, 0, 0));
            distortionCoefficients.Add(MatrixWrapperOpenCV.CvGetReal2D(ref dist_coeffs, 0, 1));
            distortionCoefficients.Add(MatrixWrapperOpenCV.CvGetReal2D(ref dist_coeffs, 0, 2));
            distortionCoefficients.Add(MatrixWrapperOpenCV.CvGetReal2D(ref dist_coeffs, 0, 3));
            intrinsicParameters.DistortionCoefficients = distortionCoefficients;

            // TBD: rot and trans storage required?

            // TBD: rot_vects, trans_vects to be released here?            
            MatrixWrapperOpenCV.CvReleaseMat(ref dist_coeffs);
            MatrixWrapperOpenCV.CvReleaseMat(ref camera_matrix);
            MatrixWrapperOpenCV.CvReleaseMat(ref point_counts);
            MatrixWrapperOpenCV.CvReleaseMat(ref image_points);
            MatrixWrapperOpenCV.CvReleaseMat(ref object_points);

            return intrinsicParameters;
        }

        private static int CvRodrigues2(ref CvMat src, ref CvMat dst, ref CvMat jacobian)
        {
            return cvRodrigues2(ref src, ref dst, ref jacobian);
        }
        [DllImport(CV_LIBRARY)]
        private static extern int cvRodrigues2(ref CvMat src, ref CvMat dst, ref CvMat jacobian);

        private static void CvFindExtrinsicCameraParams2(ref CvMat object_points, ref CvMat image_points, ref CvMat intrinsic_matrix, ref CvMat distortion_coeffs, ref CvMat rotation_vector, ref CvMat translation_vector)
        {
            cvFindExtrinsicCameraParams2(ref object_points, ref image_points, ref intrinsic_matrix, ref distortion_coeffs, ref rotation_vector, ref translation_vector);
        }
        [DllImport(CV_LIBRARY)]
        private static extern void cvFindExtrinsicCameraParams2(ref CvMat object_points, ref CvMat image_points, ref CvMat intrinsic_matrix, ref CvMat distortion_coeffs, ref CvMat rotation_vector, ref CvMat translation_vector);

        private static void CvUndistortPoints(ref CvMat src, ref CvMat dst, ref CvMat cameraMatrix, ref CvMat distCoeffs)
        {
            CvMat identity = MatrixWrapperOpenCV.CvCreateMat(3, 3, OpenCVConstants.CV_64FC1);
            MatrixWrapperOpenCV.CvSetReal2D(ref identity, 0, 0, 1);
            MatrixWrapperOpenCV.CvSetReal2D(ref identity, 1, 1, 1);
            MatrixWrapperOpenCV.CvSetReal2D(ref identity, 2, 2, 1);

            cvUndistortPoints(ref src, ref dst, ref cameraMatrix, ref distCoeffs, ref identity, ref identity);

            MatrixWrapperOpenCV.CvReleaseMat(ref identity);
        }
        [DllImport(CV_LIBRARY_210)]
        private static extern void cvUndistortPoints(ref CvMat src, ref CvMat dst, ref CvMat cameraMatrix, ref CvMat distCoeffs, ref CvMat r, ref CvMat p);

        public static IntPtr Convert1DArrToPtr(CvPoint2D32f[] arr, out GCHandle handle)
        {
            handle = new GCHandle();
            handle = GCHandle.Alloc(arr, GCHandleType.Pinned);
            return handle.AddrOfPinnedObject();
        }

        public static int CvFindChessboardCorners(ref IplImage image, CvSize pattern_size, IntPtr corners, ref int corner_count, int flags)
        {
            return cvFindChessboardCorners(ref image, pattern_size, corners, ref corner_count, flags);
        }
        [DllImport(CV_LIBRARY)]
        private static extern int cvFindChessboardCorners(ref IplImage image, CvSize pattern_size, IntPtr corners, ref int corner_count, int flags);

        public static void CvFindCornerSubPix(ref IplImage image, IntPtr corners, int count, CvSize win, CvSize zero_zone, CvTermCriteria criteria)
        {
            cvFindCornerSubPix(ref image, corners, count, win, zero_zone, criteria);
        }
        [DllImport(CV_LIBRARY)]
        private static extern void cvFindCornerSubPix(ref IplImage image, IntPtr corners, int count, CvSize win, CvSize zero_zone, CvTermCriteria criteria);

        public static void CvDrawChessboardCorners(ref IplImage image, CvSize pattern_size, IntPtr corners, int count, int pattern_was_found)
        {
            cvDrawChessboardCorners(ref image, pattern_size, corners, count, pattern_was_found);
        }
        [DllImport(CV_LIBRARY)]
        private static extern void cvDrawChessboardCorners(ref IplImage image, CvSize pattern_size, IntPtr corners, int count, int pattern_was_found);

        public static void CvCalibrateCamera2(ref CvMat object_points, ref CvMat image_points,
            ref CvMat point_counts, CvSize image_size, ref CvMat intrinsic_matrix, ref CvMat distortion_coeffs,
                                                 ref CvMat rotation_vectors, ref CvMat translation_vectors, int flags)
        {
            cvCalibrateCamera2(ref object_points, ref image_points, ref point_counts,
                image_size, ref intrinsic_matrix, ref distortion_coeffs, ref rotation_vectors,
                ref translation_vectors, flags);
        }
        [DllImport(CV_LIBRARY)]
        private static extern void cvCalibrateCamera2(ref CvMat object_points, ref CvMat image_points,
            ref CvMat point_counts, CvSize image_size, ref CvMat intrinsic_matrix, ref CvMat distortion_coeffs,
                                                 ref CvMat rotation_vectors, ref CvMat translation_vectors, int flags);        
    }
}
